// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <iostream>

// include ROS 1
#ifdef __clang__
# pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wunused-parameter"
#endif
#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#ifdef __clang__
# pragma clang diagnostic pop
#endif

// include ROS 2
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"





class ImageTrans : public rclcpp::Node
{
  public:
    ros::NodeHandle n;
    std::string m_vehicle_name;
    ros::Publisher pub;
    // int i =0;

    ImageTrans()
    : Node("img_subscriber")
    {
      this->declare_parameter("vehicle_name");
        m_vehicle_name = this->get_parameter("vehicle_name").as_string();
      
      std::string topic_name = "/" + m_vehicle_name + "/slot6/image_raw";

      pub = n.advertise<sensor_msgs::Image>(topic_name, 1);
      

      subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
      topic_name, 1, std::bind(&ImageTrans::imageCallback, this, std::placeholders::_1));
    }

  private:
    void imageCallback(const sensor_msgs::msg::Image::SharedPtr ros2_msg)
    {
      // rclcpp::RCLCPP_INFO("I heard image");
      sensor_msgs::Image ros1_msg;
      ros1_msg.header.frame_id = ros2_msg->header.frame_id;
      ros1_msg.header.stamp.sec = ros2_msg->header.stamp.sec;
      ros1_msg.header.stamp.nsec = ros2_msg->header.stamp.nanosec;
      ros1_msg.height = ros2_msg->height;
      ros1_msg.width = ros2_msg->width;
      ros1_msg.encoding = ros2_msg->encoding;
      ros1_msg.step = ros2_msg->step;
      ros1_msg.data.assign(ros2_msg->data.begin(), ros2_msg->data.end());
      
      // std::cout << "I heard image: " << i<< std::endl;
      // i++;
      // rclcpp::RCLCPP_INFO("publish this image");
      pub.publish(ros1_msg);
    }
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
};

int main(int argc, char ** argv)
{
  std::string i = std::string(argv[0]);
  // char * r_argv = i.c_str();
  int r_argc = 1;
  // std::cout<<topic_name<<std::endl;
  ros::init(r_argc, argv,"image_node",ros::init_options::AnonymousName);
  
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ImageTrans>());
  rclcpp::shutdown();
  return 0;
}